#define PCL_NO_PRECOMPILE
#include <iostream>	
#include <cmath>
#include <ros/ros.h>

#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/filters/voxel_grid.h>
#include<pcl/filters/passthrough.h>


#include "sub_sensor/rslidar_type.h"

// 如果需要使用STL算法，保留以下头文件
#include <algorithm>

class RslidarImuIn {
private:
    ros::NodeHandle nh;
    ros::Subscriber rslidar_sub;
    ros::Subscriber imu_sub;
    ros::Publisher voxel_cloud_pub;
    std::string imu_frame_id_;
    std::string lidar_frame_id_;

    pcl::PointCloud<PointType>::Ptr rslidarin;
    pcl::PointCloud<PointType>::Ptr  voxelout;
    std_msgs::Header cloudheader;
    

    public:
    RslidarImuIn():
    nh("~"){
        rslidar_sub=nh.subscribe<sensor_msgs::PointCloud2>("rslidar",10,&RslidarImuIn::cloudhander,this);
        imu_sub=nh.subscribe<sensor_msgs::Imu>("imu",10,&RslidarImuIn::imu,this);
        voxel_cloud_pub=nh.advertise<sensor_msgs::PointCloud2>("voxel_cloud",10);
        allocateMemory();
        resetParameters();
       
    }
     void allocateMemory()
     {
     pcl::PointCloud<PointType>::Ptr rslidarin(new pcl::PointCloud<PointType>());
     pcl::PointCloud<PointType>::Ptr voxelout(new pcl::PointCloud<PointType>());
     }
     
    void resetParameters()
    {
        rslidarin->clear();
        voxelout->clear();
    }

    ~RslidarImuIn(){}
    


    void cloudhander(const sensor_msgs::PointCloud2::ConstPtr& msg)
    {
      copypointcloud(msg);
      groundremove();
      voxel_filter();
      publishcloud();
    }
    void imu(const sensor_msgs::Imu::ConstPtr& msg)
    {
      
    }

    void copypointcloud(const sensor_msgs::PointCloud2::ConstPtr& lasercloudmsg)
     {
      cloudheader=lasercloudmsg->header;
      pcl::fromROSMsg(*lasercloudmsg,*rslidarin);
      std::vector<int> indices;
      pcl::removeNaNFromPointCloud(*rslidarin,*rslidarin,indices);

     }

     void groundremove()
     {

       pcl::PassThrough<PointType> pass;
       pass.setInputCloud(rslidarin);
       pass.setFilterFieldName("z");
       pass.setFilterLimits(-0.5,std::numeric_limits<float>::max());
       pass.filter(*rslidarin);

     }
   
    void voxel_filter()
    {
    pcl::VoxelGrid<PointType> sor;
    sor.setInputCloud(rslidarin);
    sor.setLeafSize(0.1,0.1,0.1);
    sor.filter(*voxelout);
    }
    void publishcloud()
    {
        sensor_msgs::PointCloud2 lasercloud;
          
        if(voxel_cloud_pub.getNumSubscribers()!=0)
        {
         pcl::toROSMsg(*voxelout,lasercloud);
         lasercloud.header.stamp=cloudheader.stamp;
         lasercloud.header.frame_id="rslidar";
         voxel_cloud_pub.publish(lasercloud);

        }
    }


};




int main(int argc, char** argv) {
    ros::init(argc, argv, "rslidar_imu_sub");
    RslidarImuIn integrator;
    ros::spin(); // 进入ROS消息处理循环
    return 0;
}